﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Globalization;

using System.Numerics;

using NumDouble = MathNet.Numerics.LinearAlgebra.Double;
using NumComplex = MathNet.Numerics.LinearAlgebra.Complex;

namespace Estimador_EDAL.Estimador
{
    public class CEstimadorDeEstadoMQP
    {
        private uint m_MaxIter;
        private double m_Tol;
        private CRedeElet m_RedeElet;
        private CMedElet m_MedElet;
        private CEstadoEsperado m_EstElet;

        private bool m_ImprimeDadosRede;
        private bool m_ImprimeValidacaoRede;
        private bool m_ImprimeDadosMed;
        private bool m_ImprimeValidacaoMed;
        private bool m_ImprimeDadosEstado;
        private bool m_ImprimeValidacaoEstado;

        private bool m_ImprimeIteracoes;
        private bool m_HabilitaMQP;

        #region Construção do Objeto

        public CEstimadorDeEstadoMQP()
        {
            Defaults();
        }

        public CEstimadorDeEstadoMQP(CEstimadorDeEstadoMQP estimador)
        {
            Copy(estimador);
        }

        public CEstimadorDeEstadoMQP(uint maxIter, double tol, CRedeElet redeElet = null, CMedElet medElet = null, CEstadoEsperado estElet = null)
        {
            Defaults();

            m_MaxIter = maxIter;
            m_Tol = tol;
            m_RedeElet = redeElet;
            m_MedElet = medElet;
            m_EstElet = estElet;
        }

        public void CarregaRedeDeArquivo(string fileName, ref string repDados, ref string repValida)
        {
            m_RedeElet = new CRedeElet();

            CultureInfo formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
            string str_Rede = System.IO.File.ReadAllText(fileName);
            m_RedeElet.CarregaDeTxt(str_Rede);
            
            if ((m_ImprimeDadosRede) && (repDados != null)) repDados += m_RedeElet.DescarregaParaTxt(repDados == "");
            if ((m_ImprimeValidacaoRede) && (repValida != null)) repValida += m_RedeElet.ValidaDados(repValida == "");
        }

        public void CarregaMedDeArquivo(string fileName, ref string repDados, ref string repValida)
        {
            m_MedElet = new CMedElet();

            string str_Med = System.IO.File.ReadAllText(fileName);
            m_MedElet.CarregaDeTxt(str_Med);

            if ((m_ImprimeDadosMed) && (repDados != null)) repDados += m_MedElet.DescarregaParaTxt(repDados == "");
            if ((m_ImprimeValidacaoMed) && (repValida != null)) repValida += m_MedElet.ValidaDados(repValida == "");
        }

        public void CarregaEstadoDeArquivo(string fileName, ref string repDados, ref string repValida)
        {
            m_EstElet = new CEstadoEsperado();

            string str_Est = System.IO.File.ReadAllText(fileName);
            m_EstElet.CarregaDeTxt(str_Est);

            if ((m_ImprimeDadosEstado) && (repDados != null)) repDados += m_EstElet.DescarregaParaTxt(repDados == "");
            if ((m_ImprimeValidacaoEstado) && (repValida != null)) repValida += m_EstElet.ValidaDados(repValida == "");
        }

        private void Defaults()
        {
            m_MaxIter = 100;
            m_Tol = 0.000001;
            m_RedeElet = null;
            m_MedElet = null;
            m_EstElet = null;

            m_ImprimeDadosRede = true;
            m_ImprimeValidacaoRede = true;
            m_ImprimeDadosMed = true;
            m_ImprimeValidacaoMed = true;
            m_ImprimeDadosEstado = true;
            m_ImprimeValidacaoEstado = true;

            m_ImprimeIteracoes = false;
            m_HabilitaMQP = true;
        }

        private void Copy(CEstimadorDeEstadoMQP estimador)
        {
            m_MaxIter = estimador.m_MaxIter;
            m_Tol = estimador.m_Tol;
            m_RedeElet = estimador.m_RedeElet;
            m_MedElet = estimador.m_MedElet;
            m_EstElet = estimador.m_EstElet;

            m_ImprimeDadosRede = estimador.m_ImprimeDadosRede;
            m_ImprimeValidacaoRede = estimador.m_ImprimeValidacaoRede;
            m_ImprimeDadosMed = estimador.m_ImprimeDadosMed;
            m_ImprimeValidacaoMed = estimador.m_ImprimeValidacaoMed;
            m_ImprimeDadosEstado = estimador.m_ImprimeDadosEstado;
            m_ImprimeValidacaoEstado = estimador.m_ImprimeValidacaoEstado;
            
            m_ImprimeIteracoes = estimador.m_ImprimeIteracoes;
            m_HabilitaMQP = estimador.m_HabilitaMQP;
        }

        #endregion Construção do Objeto

        #region Gets e Sets

        public bool ImprimeDadosRede
        {
            get { return m_ImprimeDadosRede; }
            set { if (m_ImprimeDadosRede != value) m_ImprimeDadosRede = value; }
        }

        public bool ImprimeValidacaoRede
        {
            get { return m_ImprimeValidacaoRede; }
            set { if (m_ImprimeValidacaoRede != value) m_ImprimeValidacaoRede = value; }
        }

        public bool ImprimeDadosMed
        {
            get { return m_ImprimeDadosMed; }
            set { if (m_ImprimeDadosMed != value) m_ImprimeDadosMed = value; }
        }

        public bool ImprimeValidacaoMed
        {
            get { return m_ImprimeValidacaoMed; }
            set { if (m_ImprimeValidacaoMed != value) m_ImprimeValidacaoMed = value; }
        }

        public bool ImprimeDadosEstado
        {
            get { return m_ImprimeDadosEstado; }
            set { if (m_ImprimeDadosEstado != value) m_ImprimeDadosEstado = value; }
        }

        public bool ImprimeValidacaoEstado
        {
            get { return m_ImprimeValidacaoEstado; }
            set { if (m_ImprimeValidacaoEstado != value) m_ImprimeValidacaoEstado = value; }
        }

        public bool ImprimeIteracoes
        {
            get { return m_ImprimeIteracoes; }
            set { if (m_ImprimeIteracoes != value) m_ImprimeIteracoes = value; }
        }

        public bool HabilitaMQP
        {
            get { return m_HabilitaMQP; }
            set { if (m_HabilitaMQP != value) m_HabilitaMQP = value; }
        }

        #endregion Gets e Sets

        #region MQP

        public bool Resolve(ref string report)
        {
            CultureInfo formatProvider = (CultureInfo) CultureInfo.InvariantCulture.Clone();
            formatProvider.TextInfo.ListSeparator = " ";

            if (report != null)
            {
                if (report != "") report += "\n\n";
                report += "Entrou em CEstimadorDeEstadoMQP.Resolve...\n\n";
            }

            int nroBarr = m_RedeElet.ListBarr.Count;
            int nroMeds = m_MedElet.ListMed.Count;

            //0 - inicializa z
            NumDouble.DenseVector zmed = new NumDouble.DenseVector(nroMeds, 0.0);
            for(int i = 0; i < nroMeds; i++)
                zmed[i] = m_MedElet.ListMed[i].Valor;
            if ((report != null) && (m_ImprimeIteracoes)) report += "zmed = \n" + zmed.ToString("#0.0000\t", formatProvider) + "\n\n";

            //1 - inicializa x
            NumDouble.DenseVector xatual = new NumDouble.DenseVector(2 * nroBarr - 1, 0.0);
            for(int i = 0; i < nroBarr; i++)
                xatual[i + (nroBarr - 1)] = 1.0;
            if ((report != null) && (m_ImprimeIteracoes)) report += "xatual = \n" + xatual.ToString("#0.0000\t", formatProvider) + "\n\n";

            if ((report != null) && (m_ImprimeIteracoes)) report += "Loop principal...\n\n";
            int contIter = 0;
            double diff = double.MaxValue;

            NumDouble.DenseMatrix inverseR = (m_HabilitaMQP) ? (NumDouble.DenseMatrix)m_MedElet.R.Inverse() : NumDouble.DenseMatrix.Identity(nroMeds);
            if ((report != null) && (m_ImprimeIteracoes)) report += "Inversa de R = \n" + inverseR.ToString("#0.0000\t", formatProvider) + "\n\n";

            while ((contIter++ < m_MaxIter) && (diff > m_Tol))
            {
                if ((report != null) && (m_ImprimeIteracoes)) report += "Iteração " + contIter.ToString() + ":\n\n";

                //2 - atualiza h (grandezas medidas calculadas assumindo estado definido por x)
                NumDouble.DenseVector h = Calcula_h(xatual);
                if ((report != null) && (m_ImprimeIteracoes)) report += "h = \n" + h.ToString("#0.0000\t", formatProvider) + "\n\n";

                //3 - atualiza H (Jacobiano)
                NumDouble.DenseMatrix H = Calcula_H(xatual);
                if ((report != null) && (m_ImprimeIteracoes)) report += "H = \n" + H.ToString("#0.0000\t", formatProvider) + "\n\n";

                //4 - calcula matriz G
                NumDouble.DenseMatrix transposeH = (NumDouble.DenseMatrix) H.Transpose();
                NumDouble.DenseMatrix G = transposeH * inverseR * H;
                if ((report != null) && (m_ImprimeIteracoes)) report += "G = \n" + G.ToString("#0.0000\t", formatProvider) + "\n\n";

                //5 - calcula deltax
                NumDouble.DenseMatrix inverseG = (NumDouble.DenseMatrix) G.Inverse();
                NumDouble.DenseMatrix deltaX = (NumDouble.DenseMatrix) inverseG * transposeH * inverseR * (NumDouble.DenseMatrix) ((zmed - h).ToColumnMatrix());
                diff = deltaX.Column(0).AbsoluteMaximum();
                if ((report != null) && (m_ImprimeIteracoes)) report += "deltaX = \n" + deltaX.ToString("#0.0000\t", formatProvider) + "\n\n";

                //6 - atualiza x
                xatual += (NumDouble.DenseVector) deltaX.Column(0);
                if ((report != null) && (m_ImprimeIteracoes)) report += "xatual = \n" + xatual.ToString("#0.0000\t", formatProvider) + "\n\n";
            }

            //calcula h para atualizar para a última alteração de xatual...
            NumDouble.DenseVector hfinal = Calcula_h(xatual);

            if (report != null)
            {
                //convergiu?
                if (contIter < m_MaxIter) report += "Convergiu!\n\n";
                else                      report += "Não convergiu! Atingiu o número máximo de iterações sem convergência...\n\n";

                report += "zmed = \n" + zmed.ToString("#0.0000\t", formatProvider) + "\n\n";
                report += "h = \n" + hfinal.ToString("#0.0000\t", formatProvider) + "\n\n";

                report += "xfinal = \n\n";
                report += "    Vi    Tetai\n";
                for (int k = 0; k < nroBarr; k++)
                {
                    if (k == 0) report += xatual[k + nroBarr - 1].ToString("0.0000\t", formatProvider) + "0.0000°\n";
                    else report += xatual[k + nroBarr - 1].ToString("0.0000\t", formatProvider) + (xatual[k - 1] * 180.0 / Math.PI).ToString("0.0000", formatProvider) + "°\n";
                }

                report += "\n\n" + ImprimeZmedVersusZest(zmed, hfinal);
                report += "\n\n" + ImprimeXespVersusXest(xatual);
            }

            return (contIter < m_MaxIter);
        }

        private NumDouble.DenseVector Calcula_h(NumDouble.DenseVector xest)
        {
            int nroBarr = m_RedeElet.ListBarr.Count;
            int nroMeds = m_MedElet.ListMed.Count;

            //divide xest em fase e magnitude
            NumDouble.DenseVector faseVec = new NumDouble.DenseVector(nroBarr, 0.0);
            NumDouble.DenseVector magnVec = new NumDouble.DenseVector(nroBarr, 0.0);
            for(int lin = 0; lin < nroBarr; lin++)
            {
                if (lin != 0) faseVec[lin] = xest[lin - 1];
                magnVec[lin] = xest[lin + (nroBarr - 1)];
            }
            
            //declara vetor com medições "calculadas" para estimativa xest
            NumDouble.DenseVector hret = new NumDouble.DenseVector(nroMeds, 0.0);

            //LOOP principal
            for(int lin=0; lin<nroMeds; lin++)
            {
                int posDe = m_RedeElet.ListBarr.IndexOf(m_MedElet.ListMed[lin].BarraDE);
                int posPara = m_RedeElet.ListBarr.IndexOf(m_MedElet.ListMed[lin].BarraPARA);
                
                switch(m_MedElet.ListMed[lin].Tipo)
                {
                    case CMedicaoType.Pi:
                        {
                            for(int col=0; col<nroBarr; col++)
                            {
                                double Vj = magnVec[col];
                                double Gij = m_RedeElet.Ybus[posDe,col].Real;
                                double Bij = m_RedeElet.Ybus[posDe,col].Imaginary;
                                double Tetaij = faseVec[posDe] - faseVec[col];
                                hret[lin] += Vj * (Gij * Math.Cos(Tetaij) + Bij * Math.Sin(Tetaij));
                            }
                            double Vi = magnVec[posDe];
                            hret[lin] *= Vi;
                            break;
                        }
                    case CMedicaoType.Pij:
                        {
                            double Vi = magnVec[posDe];
                            double Vj = magnVec[posPara];
                            double Gij = m_RedeElet.Ybus[posDe,posPara].Real;
                            double Bij = m_RedeElet.Ybus[posDe,posPara].Imaginary;
                            double Tetaij = faseVec[posDe] - faseVec[posPara];
                            hret[lin] = Vi * Vi * (-Gij) - Vi * Vj *(-Gij * Math.Cos(Tetaij) - Bij * Math.Sin(Tetaij));
                            break;
                        }
                    case CMedicaoType.Qi:
                        {
                            for(int col=0; col<nroBarr; col++)
                            {
                                double Vj = magnVec[col];
                                double Gij = m_RedeElet.Ybus[posDe,col].Real;
                                double Bij = m_RedeElet.Ybus[posDe,col].Imaginary;
                                double Tetaij = faseVec[posDe] - faseVec[col];
                                hret[lin] += Vj * (Gij * Math.Sin(Tetaij) - Bij * Math.Cos(Tetaij));
                            }
                            double Vi = magnVec[posDe];
                            hret[lin] *= Vi;
                            break;
                        }
                    case CMedicaoType.Qij:
                        {
                            double Vi = magnVec[posDe];
                            double Vj = magnVec[posPara];
                            double Gij = m_RedeElet.Ybus[posDe,posPara].Real;
                            double Bij = m_RedeElet.Ybus[posDe,posPara].Imaginary;
                            double Tetaij = faseVec[posDe] - faseVec[posPara];
                            hret[lin] = -Vi * Vi * (-Bij) - Vi * Vj * (-Gij*Math.Sin(Tetaij) + Bij * Math.Cos(Tetaij));
                            break;
                        }
                    case CMedicaoType.Iij:
                        {
                            double Vi = magnVec[posDe];
                            double Vj = magnVec[posPara];
                            double Gij = m_RedeElet.Ybus[posDe,posPara].Real;
                            double Bij = m_RedeElet.Ybus[posDe,posPara].Imaginary;
                            double Tetaij = faseVec[posDe] - faseVec[posPara];
                            double Pij = Vi * Vi * (-Gij) - Vi * Vj *(-Gij*Math.Cos(Tetaij) - Bij*Math.Sin(Tetaij));
                            double Qij = -Vi * Vi * (-Bij) - Vi * Vj * (-Gij*Math.Sin(Tetaij) + Bij * Math.Cos(Tetaij));
                            hret[lin] = Math.Sqrt(Pij * Pij + Qij * Qij) / Vi;
                            break;
                        }
                    case CMedicaoType.Vi:
                        {
                            double Vi = magnVec[posDe];
                            hret[lin] = Vi;
                            break;
                        }
                    default:
                        {
                            System.Diagnostics.Debug.WriteLine("Falha (CEstimadorDeEstadoMQP.Calcula_h): ocorreu medição com tipo Empty.");
                            hret[lin] = 0;
                            break;
                        }
                }
            }
            return hret;
        }

        private NumDouble.DenseMatrix Calcula_H(NumDouble.DenseVector xest)
        {
            int nroBarr = m_RedeElet.ListBarr.Count;
            int nroMeds = m_MedElet.ListMed.Count;

            //divide xest em fase e magnitude
            NumDouble.DenseVector faseVec = new NumDouble.DenseVector(nroBarr, 0.0);
            NumDouble.DenseVector magnVec = new NumDouble.DenseVector(nroBarr, 0.0);
            for (int lin = 0; lin < nroBarr; lin++)
            {
                if (lin != 0) faseVec[lin] = xest[lin - 1];
                magnVec[lin] = xest[lin + (nroBarr - 1)];
            }

            //declara vetor com medições "calculadas" para estimativa xest
            NumDouble.DenseMatrix Hjac = new NumDouble.DenseMatrix(nroMeds, 2 * nroBarr - 1, 0.0);

            //LOOP principal 1: derivadas em relação a Teta
            for (int lin = 0; lin < nroMeds; lin++)
            {
                for (int col = 0; col < (nroBarr - 1); col++)
                {
                    int posDe = m_RedeElet.ListBarr.IndexOf(m_MedElet.ListMed[lin].BarraDE);
                    int posPara = m_RedeElet.ListBarr.IndexOf(m_MedElet.ListMed[lin].BarraPARA);

                    switch (m_MedElet.ListMed[lin].Tipo)
                    {
                        case CMedicaoType.Pi:
                            {
                                if (posDe == col + 1)
                                {
                                    double Vi = magnVec[posDe];
                                    for (int j = 0; j < nroBarr; j++)
                                    {
                                        double Vj = magnVec[j];
                                        double Gij = m_RedeElet.Ybus[posDe, j].Real;
                                        double Bij = m_RedeElet.Ybus[posDe, j].Imaginary;
                                        double Tetaij = faseVec[posDe] - faseVec[j];
                                        Hjac[lin, col] += Vi * Vj * (-Gij * Math.Sin(Tetaij) + Bij * Math.Cos(Tetaij));
                                    }
                                    double Bii = m_RedeElet.Ybus[posDe, posDe].Imaginary;
                                    Hjac[lin, col] += -Vi * Vi * Bii;
                                }
                                else
                                {
                                    double Vi = magnVec[posDe];
                                    double Vj = magnVec[col + 1];
                                    double Gij = m_RedeElet.Ybus[posDe, col + 1].Real;
                                    double Bij = m_RedeElet.Ybus[posDe, col + 1].Imaginary;
                                    double Tetaij = faseVec[posDe] - faseVec[col + 1];
                                    Hjac[lin, col] += Vi * Vj * (Gij * Math.Sin(Tetaij) - Bij * Math.Cos(Tetaij));
                                }
                                break;
                            }
                        case CMedicaoType.Pij:
                            {
                                double Vi = magnVec[posDe];
                                double Vj = magnVec[posPara];
                                double Gij = m_RedeElet.Ybus[posDe, posPara].Real;
                                double Bij = m_RedeElet.Ybus[posDe, posPara].Imaginary;
                                double Tetaij = faseVec[posDe] - faseVec[posPara];

                                if (posDe == col + 1)
                                    Hjac[lin, col] += Vi * Vj * (-Gij * Math.Sin(Tetaij) + Bij * Math.Cos(Tetaij));
                                else if (posPara == col + 1)
                                    Hjac[lin, col] += -Vi * Vj * (-Gij * Math.Sin(Tetaij) + Bij * Math.Cos(Tetaij));
                                break;
                            }
                        case CMedicaoType.Qi:
                            {
                                if (posDe == col + 1)
                                {
                                    double Vi = magnVec[posDe];
                                    for (int j = 0; j < nroBarr; j++)
                                    {
                                        double Vj = magnVec[j];
                                        double Gij = m_RedeElet.Ybus[posDe, j].Real;
                                        double Bij = m_RedeElet.Ybus[posDe, j].Imaginary;
                                        double Tetaij = faseVec[posDe] - faseVec[j];
                                        Hjac[lin, col] += Vi * Vj * (Gij * Math.Cos(Tetaij) + Bij * Math.Sin(Tetaij));
                                    }
                                    double Gii = m_RedeElet.Ybus[posDe, posDe].Real;
                                    Hjac[lin, col] += -Vi * Vi * Gii;
                                }
                                else
                                {
                                    double Vi = magnVec[posDe];
                                    double Vj = magnVec[col + 1];
                                    double Gij = m_RedeElet.Ybus[posDe, col + 1].Real;
                                    double Bij = m_RedeElet.Ybus[posDe, col + 1].Imaginary;
                                    double Tetaij = faseVec[posDe] - faseVec[col + 1];
                                    Hjac[lin, col] += Vi * Vj * (-Gij * Math.Cos(Tetaij) - Bij * Math.Sin(Tetaij));
                                }
                                break;
                            }
                        case CMedicaoType.Qij:
                            {
                                double Vi = magnVec[posDe];
                                double Vj = magnVec[posPara];
                                double Gij = m_RedeElet.Ybus[posDe, posPara].Real;
                                double Bij = m_RedeElet.Ybus[posDe, posPara].Imaginary;
                                double Tetaij = faseVec[posDe] - faseVec[posPara];

                                if (posDe == col + 1)
                                    Hjac[lin, col] += -Vi * Vj * (Gij * Math.Cos(Tetaij) + Bij * Math.Sin(Tetaij));
                                else if (posPara == col + 1)
                                    Hjac[lin, col] += -Vi * Vj * (Gij * Math.Cos(Tetaij) - Bij * Math.Sin(Tetaij));
                                break;
                            }
                        case CMedicaoType.Iij:
                            {
                                //Esse não tinha modelo em Matlab... 
                                //fiz com base no livro do Expósito, em que desconsidera corrente pelo susceptância shunt da LT.

                                double Vi = magnVec[posDe];
                                double Vj = magnVec[posPara];
                                double Gij = m_RedeElet.Ybus[posDe, posPara].Real;
                                double Bij = m_RedeElet.Ybus[posDe, posPara].Imaginary;
                                double Tetaij = faseVec[posDe] - faseVec[posPara];

                                if (posDe == col + 1)
                                {
                                    double GGBB = Gij * Gij + Bij * Bij;
                                    double Iij = Math.Sqrt(GGBB * (Vi * Vi + Vj * Vj - 2 * Vi * Vj * Math.Cos(Tetaij)));
                                    Hjac[lin, col] += GGBB * Vi * Vj * Math.Sin(Tetaij) / Iij;
                                }
                                else if (posPara == col + 1)
                                {
                                    double GGBB = Gij * Gij + Bij * Bij;
                                    double Iij = Math.Sqrt(GGBB * (Vi * Vi + Vj * Vj - 2 * Vi * Vj * Math.Cos(Tetaij)));
                                    Hjac[lin, col] += -GGBB * Vi * Vj * Math.Sin(Tetaij) / Iij;
                                }
                                break;
                            }
                        case CMedicaoType.Vi:
                            {
                                //nada a fazer... Hjac[lin, col] permanece "zero".
                                break;
                            }
                        default:
                            {
                                System.Diagnostics.Debug.WriteLine("Falha (CEstimadorDeEstadoMQP.Calcula_H): ocorreu medição com tipo Empty.");
                                break;
                            }
                    }
                }
            }

            //LOOP principal 2: derivadas em relação a Magnitude
            for (int lin = 0; lin < nroMeds; lin++)
            {
                for (int col = 0; col < nroBarr; col++)
                {
                    int colCorr = col + nroBarr - 1;

                    int posDe = m_RedeElet.ListBarr.IndexOf(m_MedElet.ListMed[lin].BarraDE);
                    int posPara = m_RedeElet.ListBarr.IndexOf(m_MedElet.ListMed[lin].BarraPARA);

                    switch (m_MedElet.ListMed[lin].Tipo)
                    {
                        case CMedicaoType.Pi:
                            {
                                if (posDe == col)
                                {
                                    for (int j = 0; j < nroBarr; j++)
                                    {
                                        double Vj = magnVec[j];
                                        double Gij = m_RedeElet.Ybus[posDe, j].Real;
                                        double Bij = m_RedeElet.Ybus[posDe, j].Imaginary;
                                        double Tetaij = faseVec[posDe] - faseVec[j];
                                        Hjac[lin, colCorr] += Vj * (Gij * Math.Cos(Tetaij) + Bij * Math.Sin(Tetaij));
                                    }
                                    double Vi = magnVec[posDe];
                                    double Gii = m_RedeElet.Ybus[posDe, posDe].Real;
                                    Hjac[lin, colCorr] += Vi * Gii;
                                }
                                else
                                {
                                    double Vi = magnVec[posDe];
                                    double Vj = magnVec[col];
                                    double Gij = m_RedeElet.Ybus[posDe, col].Real;
                                    double Bij = m_RedeElet.Ybus[posDe, col].Imaginary;
                                    double Tetaij = faseVec[posDe] - faseVec[col];
                                    Hjac[lin, colCorr] += Vi * (Gij * Math.Cos(Tetaij) + Bij * Math.Sin(Tetaij));
                                }
                                break;
                            }
                        case CMedicaoType.Pij:
                            {
                                double Vi = magnVec[posDe];
                                double Vj = magnVec[posPara];
                                double Gij = m_RedeElet.Ybus[posDe, posPara].Real;
                                double Bij = m_RedeElet.Ybus[posDe, posPara].Imaginary;
                                double Tetaij = faseVec[posDe] - faseVec[posPara];

                                if (posDe == col)
                                    Hjac[lin, colCorr] += -Vj * (-Gij * Math.Cos(Tetaij) - Bij * Math.Sin(Tetaij)) + 2 * (-Gij * Vi);
                                else if (posPara == col)
                                    Hjac[lin, colCorr] += -Vi * (-Gij * Math.Cos(Tetaij) - Bij * Math.Sin(Tetaij));
                                break;
                            }
                        case CMedicaoType.Qi:
                            {
                                if (posDe == col)
                                {
                                    for (int j = 0; j < nroBarr; j++)
                                    {
                                        double Vj = magnVec[j];
                                        double Gij = m_RedeElet.Ybus[posDe, j].Real;
                                        double Bij = m_RedeElet.Ybus[posDe, j].Imaginary;
                                        double Tetaij = faseVec[posDe] - faseVec[j];
                                        Hjac[lin, colCorr] += Vj * (Gij * Math.Sin(Tetaij) - Bij * Math.Cos(Tetaij));
                                    }
                                    double Vi = magnVec[posDe];
                                    double Bii = m_RedeElet.Ybus[posDe, posDe].Imaginary;
                                    Hjac[lin, colCorr] += -Vi * Bii;
                                }
                                else
                                {
                                    double Vi = magnVec[posDe];
                                    double Vj = magnVec[col];
                                    double Gij = m_RedeElet.Ybus[posDe, col].Real;
                                    double Bij = m_RedeElet.Ybus[posDe, col].Imaginary;
                                    double Tetaij = faseVec[posDe] - faseVec[col];
                                    Hjac[lin, colCorr] += Vi * (Gij * Math.Sin(Tetaij) - Bij * Math.Cos(Tetaij));
                                }
                                break;
                            }
                        case CMedicaoType.Qij:
                            {
                                double Vi = magnVec[posDe];
                                double Vj = magnVec[posPara];
                                double Gij = m_RedeElet.Ybus[posDe, posPara].Real;
                                double Bij = m_RedeElet.Ybus[posDe, posPara].Imaginary;
                                double Tetaij = faseVec[posDe] - faseVec[posPara];

                                if (posDe == col)
                                    Hjac[lin, colCorr] += -Vj * (-Gij * Math.Sin(Tetaij) + Bij * Math.Cos(Tetaij)) - 2 * (-Bij * Vi);
                                else if (posPara == col)
                                    Hjac[lin, colCorr] += -Vi * (-Gij * Math.Sin(Tetaij) + Bij * Math.Cos(Tetaij));
                                break;
                            }
                        case CMedicaoType.Iij:
                            {
                                //Esse não tinha modelo em Matlab... 
                                //fiz com base no livro do Expósito, em que desconsidera corrente pelo susceptância shunt da LT.

                                double Vi = magnVec[posDe];
                                double Vj = magnVec[posPara];
                                double Gij = m_RedeElet.Ybus[posDe, posPara].Real;
                                double Bij = m_RedeElet.Ybus[posDe, posPara].Imaginary;
                                double Tetaij = faseVec[posDe] - faseVec[posPara];

                                if (posDe == col)
                                {
                                    double GGBB = Gij * Gij + Bij * Bij;
                                    double Iij = Math.Sqrt(GGBB * (Vi * Vi + Vj * Vj - 2 * Vi * Vj * Math.Cos(Tetaij)));
                                    Hjac[lin, colCorr] += GGBB * (Vi - Vj * Math.Cos(Tetaij)) / Iij;
                                }
                                else if (posPara == col)
                                {
                                    double GGBB = Gij * Gij + Bij * Bij;
                                    double Iij = Math.Sqrt(GGBB * (Vi * Vi + Vj * Vj - 2 * Vi * Vj * Math.Cos(Tetaij)));
                                    Hjac[lin, colCorr] += GGBB * (Vj - Vi * Math.Cos(Tetaij)) / Iij;
                                }
                                break;
                            }
                        case CMedicaoType.Vi:
                            {
                                if (posDe == col)
                                    Hjac[lin, colCorr] += 1.0;
                                //else
                                    //nada a fazer... Hjac[lin, col] permanece "zero".
                                break;
                            }
                        default:
                            {
                                System.Diagnostics.Debug.WriteLine("Falha (CEstimadorDeEstadoMQP.Calcula_H): ocorreu medição com tipo Empty.");
                                break;
                            }
                    }
                }
            }
            return Hjac;
        }

        private string ImprimeZmedVersusZest(NumDouble.DenseVector zmed, NumDouble.DenseVector zest)
        {
            CultureInfo formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
            formatProvider.TextInfo.ListSeparator = " ";

            string ret = "    Zmed    Zest     err    err2\n";

            double acc = 0;
            for (int i = 0; i < zmed.Count; i++)
            {
                double d_covar2 = (m_HabilitaMQP) ? m_MedElet.ListMed[i].CoVar * m_MedElet.ListMed[i].CoVar : 1.0;

                ret += string.Format(formatProvider, "{0,8:0.000}", zmed[i]);
                ret += string.Format(formatProvider, "{0,8:0.000}", zest[i]);
                ret += string.Format(formatProvider, "{0,8:0.000}", (zmed[i] - zest[i]));
                ret += string.Format(formatProvider, "{0,8:0.000}", (zmed[i] - zest[i]) * (zmed[i] - zest[i])) + "\n";

                acc += (zmed[i] - zest[i]) * (zmed[i] - zest[i]) / d_covar2;
            }
            ret += "\nf = " + acc.ToString("0.000", formatProvider);

            return ret;
        }

        private string ImprimeXespVersusXest(NumDouble.DenseVector xest)
        {
            CultureInfo formatProvider = (CultureInfo)CultureInfo.InvariantCulture.Clone();
            formatProvider.TextInfo.ListSeparator = " ";

            NumDouble.DenseVector xesperado = m_EstElet.Atualiza_X(m_RedeElet);
            NumDouble.DenseVector erro = xesperado - xest;

            string ret = "    Xesp    Xest     err    err2\n";

            double acc = 0;
            for (int i = 0; i < xest.Count; i++)
            {
                ret += string.Format(formatProvider, "{0,8:0.000}", xesperado[i]);
                ret += string.Format(formatProvider, "{0,8:0.000}", xest[i]);
                ret += string.Format(formatProvider, "{0,8:0.000}", erro[i]);
                ret += string.Format(formatProvider, "{0,8:0.000}", erro[i] * erro[i]) + "\n";

                acc += erro[i] * erro[i];
            }
            ret += "\nf = " + acc.ToString("0.000", formatProvider);

            return ret;
        }

        #endregion MQP
    }
}
